Implémentation d'un bloc fonctionnel de prétraitement de chemin

L'exemple montre une application de découpe 2D. Un couteau doit être utilisé pour couper le long d'un chemin défini par le code G. SMC_TRAFO_GantryCutter2 Une transformation est utilisée pour déterminer l'angle du couteau en fonction de la tangente actuelle de la trajectoire.

Le défi est que le couteau ne doit pas tourner trop vite, sinon la coupe ne sera pas nette. La limitation de la vitesse angulaire du couteau est résolue dans le projet d'exemple au moyen du bloc fonctionnel de prétraitement de trajectoire nouvellement créé LimitTangentVelocity. Il modifie la vitesse d'avance sur la trajectoire afin que la vitesse de variation de la tangente ne dépasse pas une valeur maximale configurable

Le code G est un simple rectangle aux coins lissés.

Le tracé montre le résultat de l'exécution : la vitesse de l'axe C, qui correspond à la vitesse de rotation du couteau, est affichée en orange. Elle est limitée à 45°/s comme spécifié

La structure est typique des applications CNC. Le code G est lu dans la tâche d'arrière-plan et le prétraitement du chemin a également lieu ici. L'interpolation est effectuée dans la tâche du bus

Le LimitTangentVelocity Un bloc fonctionnel affiche les étapes et les états nécessaires pour un bloc fonctionnel de prétraitement de chemin.

Le principe de base est que le bloc fonctionnel lit, traite puis écrit les éléments du chemin (type SMC_GEOINFO) depuis la file d'entrée (poqDataIn) vers la file d'attente de sortie (poqDataOut). Les éléments sont supprimés de la file d'attente d'entrée.

La logique permettant de modifier le débit d'alimentation se trouve dans les lignes 102 à 138. La fonction auxiliaire ComputeMaxCurvature calcule la courbure maximale qui peut apparaître sur un élément de trajectoire donné.

// Our velocity limitation comes here. This modifies an element from the
// input queue and copies the modified element to the output queue.
// Finally, the element is removed from the input queue

m_geo := pgeo^; // Copy the element
 // Note: the feature flags set in the G-Code with G38/G39 can be queried
 // by reading pgo^.dwFeatureFlags, like this:
 IF (SHR(m_geo.dwFeatureFlags, featureFlag) AND 1) = 1 THEN
     // feature is turned on

     ok := ComputeMaxCurvature(m_geo, kappa=> kappa_max);
     IF NOT ok THEN
         // Curvature cannot be computed
         m_state := STATE_ERROR;
         ErrorID := SMC_INVALID_PARAMETER;
         OnExit();
         RETURN;
     END_IF

     // Compute maximum allowed path velocity based on maximum curvature
     // and maximum allowed angular velocity
     IF kappa_max = 0 THEN
         // No curvature, no limitation necessary
         vel := m_geo.dVel;
     ELSE
         vel := m_maxAngularVelocity_rad / kappa_max;
     END_IF

     IF velMin < 0 OR vel < velMin THEN
         velMin := vel;
     END_IF

     // Set new maximum velocity for the element
     m_geo.dVel := MIN(m_geo.dVel, vel);
 ELSE
     // feature is turned off
 END_IF